#include <math.h>
#include "so_filter.h"

#define PI 3.14159265358979323846

#pragma CODE_SECTION(SO_FILTER_INIT,"ramfuncs");
void SO_FILTER_INIT(SO_FILTER *pHandle)
{
	float wa = 2.0 * PI * pHandle->cutoff_freq;  // Analog frequency in rad/s
	float alpha = wa * pHandle->ts / 2.0;
	float alpha_sqr = alpha * alpha;
	float tmp_term = sqrtf(2.0) * alpha + alpha_sqr;
	float gain = alpha_sqr / (1.0 + tmp_term);

	pHandle->denominators[0] = 1.0;
	pHandle->denominators[1] = 2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term);
	pHandle->denominators[2] = ((1.0 - sqrtf(2.0) * alpha + alpha_sqr) / (1.0 + tmp_term));

	pHandle->numerators[0] = gain;
	pHandle->numerators[1] = 2.0 * gain;
	pHandle->numerators[2] = gain;

	pHandle->x_value[0] = 0.F;
	pHandle->x_value[1] = 0.F;
	pHandle->x_value[2] = 0.F;
	pHandle->y_value[0] = 0.F;
	pHandle->y_value[1] = 0.F;
	pHandle->y_value[2] = 0.F;
}

#pragma CODE_SECTION(SO_FILTER_UPDATE,"ramfuncs");
void SO_FILTER_UPDATE(SO_FILTER *pHandle)
{
	pHandle->x_value[0] = pHandle->in;
	pHandle->y_value[0] = (pHandle->x_value[0] * pHandle->numerators[0]
							+ pHandle->x_value[1] * pHandle->numerators[1]
							+ pHandle->x_value[2] * pHandle->numerators[2]
							- pHandle->y_value[1] * pHandle->denominators[1]
							- pHandle->y_value[2] * pHandle->denominators[2])
							/ pHandle->denominators[0];

	pHandle->x_value[2] = pHandle->x_value[1];
	pHandle->x_value[1] = pHandle->x_value[0];
	pHandle->y_value[2] = pHandle->y_value[1];
	pHandle->y_value[1] = pHandle->y_value[0];
	pHandle->out = pHandle->y_value[0];
}


